﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Media.Media3D;

using CMath = System.Math;

namespace Red.Core.Math
{

    /// <summary>
    /// Euler angles implementation
    /// </summary>
    public class EulerAngles
    {
        #region Properties

        public double Roll { get; set; }
        public double Pitch { get; set; }
        public double Yaw { get; set; }

        #endregion Properties

        #region C-tor

        public EulerAngles( double roll, double pitch, double yaw )
        {
            Roll = roll;
            Pitch = pitch;
            Yaw = yaw;
        }

        #endregion C-tor

        #region Public methods

        public Matrix3D ToMatrix()
        {
            double cosRoll = CMath.Cos( DegreesToRadians( Roll ) );
            double sinRoll = CMath.Sin( DegreesToRadians( Roll ) );
            double cosPitch = CMath.Cos( DegreesToRadians( Pitch ) );
            double sinPitch = CMath.Sin( DegreesToRadians( Pitch ) );
            double cosYaw = CMath.Cos( DegreesToRadians( Yaw ) );
            double sinYaw = CMath.Sin( DegreesToRadians( Yaw ) );

            Matrix3D ret = Matrix3D.Identity;
            ret.M11 = cosRoll * cosYaw - sinPitch * sinRoll * sinYaw;
            ret.M12 = sinPitch * sinRoll * cosYaw + cosRoll * sinYaw;
            ret.M13 = -cosPitch * sinRoll;
            ret.M21 = cosPitch * -sinYaw;
            ret.M22 = cosPitch * cosYaw;
            ret.M23 = sinPitch;
            ret.M31 = sinPitch * cosRoll * sinYaw + sinRoll * cosYaw;
            ret.M32 = sinRoll * sinYaw - sinPitch * cosRoll * cosYaw;
            ret.M33 = cosPitch * cosRoll;
            return ret;
        }

        #endregion Public methods

        #region Private methods

        private double DegreesToRadians( double degrees )
        {
            return ( degrees * CMath.PI ) / 180.0;
        }

        #endregion Private methods
    }
}
